/**
 * Snowflake MEGA sumo robot application to select a one of the system tests
 * Copyright 2013 Maris BECS <maris.becs@gmail.com>
 */

#include <asf.h>
#include "keyboard.h"
#include "sensors.h"
#include "motors.h"
#include "system_tests.h"
#include "system_tests/test0.h"
#include "system_tests/test1.h"
#include "system_tests/test2.h"
#include "system_tests/test3.h"
//#include "system_tests/test4.h"
//#include "system_tests/test5.h"
//#include "system_tests/test6.h"


void system_tests_application(void)
{
	uint8_t test;
	struct keyboard_event input;

	// Clear screen
	gfx_mono_draw_filled_rect(0, 0, 128, 32, GFX_PIXEL_CLR);

	// Enable kill switch - press "8" on IR remote control to kill
	ioport_set_pin_low(START_SW);

	// Show constant label strategy selected
	gfx_mono_draw_string("TEST:", 92, 0, &sysfont);
	
	// Read & Show strategy number(0-F) from the rotary switch at top right corner
	test = rotary_read();


	while (true) {
		bool enable_test_start = false;

		if ( LOW == ioport_get_pin_level(KILL_SW) ){
			// Kill switch active
			drive(0,0); // break motors
			delay_ms(100);
			stop_motors();  // free motors
		}

		// check key press
		keyboard_get_key_state(&input);
		if (input.type == KEYBOARD_RELEASE) {
			// BACK to exit FIGHT menu
			if (input.keycode == KEYBOARD_BACK) {
				// set IR module back to OFF state
				ioport_set_pin_high(START_SW);
				return;
			// ENTER to start the selected strategy
			} else if (input.keycode == KEYBOARD_ENTER) {
				enable_test_start = true;
				delay_ms(300);	// just in case
			// DOWN to read the strategy number(0-F) from the rotary switch
			} else if (input.keycode == KEYBOARD_DOWN) {
				test = rotary_read();
			}
		}

		if (0 == test) {
			// simple drive straight test, distance sensors disabled
			gfx_mono_draw_string("Drive straight and   ", 0, 14, &sysfont);
			gfx_mono_draw_string("     stop at the line", 0, 24, &sysfont);
			if (enable_test_start) {
				test0_app();
				gfx_mono_draw_string("     TEST KILLED     ", 0, 0, &sysfont);
			}

		} else if (1 == test) {
			// simple turn test using delays, distance sensors disabled
			gfx_mono_draw_string("Turn   LEFT or RIGHT ", 0, 14, &sysfont);
			gfx_mono_draw_string("                     ", 0, 24, &sysfont);
			if (enable_test_start) {
				test1_app();
				gfx_mono_draw_string("     TEST KILLED     ", 0, 0, &sysfont);
			}

		} else if (2 == test) {
			// drive straight, distance sensors enabled
			// stop and turn at the line
			gfx_mono_draw_string("Drv strght attack if ", 0, 14, &sysfont);
			gfx_mono_draw_string(" see turn at the line", 0, 24, &sysfont);
			if (enable_test_start) {
				test2_app();
				gfx_mono_draw_string("     TEST KILLED     ", 0, 0, &sysfont);
			}

		} else if (3 == test) {
			// simple see attack test fight on selected speed (30%, 50% or 100% PWM) using delays
			// first move is to drive straight
			gfx_mono_draw_string("Drv strght search the", 0, 14, &sysfont);
			gfx_mono_draw_string(" ring & attack if see", 0, 24, &sysfont);
			if (enable_test_start) {
				test3_app();
				gfx_mono_draw_string("     TEST KILLED     ", 0, 0, &sysfont);
			}

		} else if (4 == test) {
			// Clear screen and show "NOT YET IMPLEMENTED" info
			gfx_mono_draw_filled_rect(0, 0, 128, 32, GFX_PIXEL_CLR);
			gfx_mono_draw_string(" NOT YET IMPLEMENTED ", 0, 24, &sysfont);

			/*/ todo: simple drive straight test using encoders, distance sensors disabled
			gfx_mono_draw_string("Drive straight using ", 0, 14, &sysfont);
			gfx_mono_draw_string("             encoders", 0, 24, &sysfont);
			if (enable_test_start) {
				test4_app();
				gfx_mono_draw_string("     TEST KILLED     ", 0, 0, &sysfont);
			}*/

		} else if (5 == test) {
			// Clear screen and show "NOT YET IMPLEMENTED" info
			gfx_mono_draw_filled_rect(0, 0, 128, 32, GFX_PIXEL_CLR);
			gfx_mono_draw_string(" NOT YET IMPLEMENTED ", 0, 24, &sysfont);

			/*/ todo: simple turn test using encoders, distance sensors disabled
			gfx_mono_draw_string("                     ", 0, 14, &sysfont);
			gfx_mono_draw_string("                     ", 0, 24, &sysfont);
			if (enable_test_start) {
				test5_app();
				gfx_mono_draw_string("     TEST KILLED     ", 0, 0, &sysfont);
			}*/

		} else if (6 == test) {
			// Clear screen and show "NOT YET IMPLEMENTED" info
			gfx_mono_draw_filled_rect(0, 0, 128, 32, GFX_PIXEL_CLR);
			gfx_mono_draw_string(" NOT YET IMPLEMENTED ", 0, 24, &sysfont);

			/*/ todo: implement test 6
			gfx_mono_draw_string("                     ", 0, 14, &sysfont);
			gfx_mono_draw_string("                     ", 0, 24, &sysfont);
			if (enable_test_start) {
				test6_app();
				gfx_mono_draw_string("     TEST KILLED     ", 0, 0, &sysfont);
			}*/

		}


	}
}	